/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "AnoPTv8Cmd.h"
#include "drv_pwm.h"
#include "AnoPTv8Par.h"
#include "rtthread.h"
#include "fc_state.h"

#include <stdbool.h>

#define DBG_TAG "ano/cmd"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

static bool ano_motor_on_flag = 0;
int16_t ano_motor_value[4] = {0};

int16_t ano_motor_value_get(uint8_t motor_index)
{
    return ano_motor_value[motor_index % 4];
}
void ano_motor_value_set(uint8_t motor_index, int16_t motor_value)
{
    ano_motor_value[motor_index % 4] = motor_value;
}
bool is_ano_motor_on(void)
{

    return ano_motor_on_flag;
}

void ano_motor_on(void *p)
{
    (void)p;
    ano_motor_on_flag = 1;

    ano_motor_value[0] = 0;
    ano_motor_value[1] = 0;
    ano_motor_value[2] = 0;
    ano_motor_value[3] = 0;

    motor_led_state_set(0, RGB_STATE_MODE_DOT);
    motor_led_state_set(1, RGB_STATE_MODE_DOT);
    motor_led_state_set(2, RGB_STATE_MODE_DOT);
    motor_led_state_set(3, RGB_STATE_MODE_DOT);
}
void ano_motor_off(void *p)
{
    (void)p;
    ano_motor_on_flag = 0;

    ano_motor_value[0] = 0;
    ano_motor_value[1] = 0;
    ano_motor_value[2] = 0;
    ano_motor_value[3] = 0;

    motor_led_state_set(0, RGB_STATE_MODE_BREATH);
    motor_led_state_set(1, RGB_STATE_MODE_BREATH);
    motor_led_state_set(2, RGB_STATE_MODE_BREATH);
    motor_led_state_set(3, RGB_STATE_MODE_BREATH);
}

void ano_cmd_virtaul_stick(void *p)
{
    _st_cmd_info *cmd = (_st_cmd_info *)p;
    int16_t value1 = cmd->cmd.cmdval0 | (cmd->cmd.cmdval1 << 8);
    int16_t value2 = cmd->cmd.cmdval2 | (cmd->cmd.cmdval3 << 8);
    if (value1 < 0)
    {
        ano_motor_value[0] = -value1;
        motor_led_state_set(0, RGB_STATE_MODE_FAST);
    }
    else
    {
        ano_motor_value[1] = value1;
        motor_led_state_set(1, RGB_STATE_MODE_FAST);
    }
    if (value2 < 0)
    {
        ano_motor_value[2] = -value2;
        motor_led_state_set(2, RGB_STATE_MODE_FAST);
    }
    else
    {
        ano_motor_value[3] = value2;
        motor_led_state_set(3, RGB_STATE_MODE_FAST);
    }

    if (value1 == 0)
    {
        ano_motor_value[0] = 0;
        ano_motor_value[1] = 0;
        motor_led_state_set(0, RGB_STATE_MODE_DOT);
        motor_led_state_set(1, RGB_STATE_MODE_DOT);

    }
    if (value2 == 0)
    {
        ano_motor_value[2] = 0;
        ano_motor_value[3] = 0;
        motor_led_state_set(2, RGB_STATE_MODE_DOT);
        motor_led_state_set(3, RGB_STATE_MODE_DOT);
    }

}

void ano_motor_set(void *p)
{
    _st_cmd_info *cmd = (_st_cmd_info *)p;
    uint8_t motor_index = cmd->cmd.cmdval0 % 4;
    uint16_t motor_duty = cmd->cmd.cmdval1 % 100 * 10; // 0~100% -> 0~1000
    // LOG_D("motor_index:%d,motor_duty:%d");
    // motor_set_duty(motor_index, motor_duty);
    ano_motor_value[motor_index] = motor_duty;

    if (motor_duty == 0)
    {
        motor_led_state_set(motor_index, RGB_STATE_MODE_DOT);
    }
    else
    {
        motor_led_state_set(motor_index, RGB_STATE_MODE_FAST);
    }
}
const _st_cmd_info ano_motor_on_cmd = {
    .cmd.cmdid0 = 0,
    .cmd.cmdid1 = 0,
    .cmd.cmdid2 = 1,
    .name = "motor_on",
    .info = "motor on",
    .pFun = ano_motor_on,
};
const _st_cmd_info ano_motor_off_cmd = {
    .cmd.cmdid0 = 0,
    .cmd.cmdid1 = 0,
    .cmd.cmdid2 = 2,
    .name = "motor_off",
    .info = "motor off",
    .pFun = ano_motor_off,
};
const _st_cmd_info ano_motor_set_cmd = {
    .cmd.cmdid0 = 0,
    .cmd.cmdid1 = 0,
    .cmd.cmdid2 = 3,
    .name = "motor_set",
    .info = "VAL0: motor_index (0~3), VAL1: motor_duty(0~100%)", // 0~100% -> 0~1000
    .pFun = ano_motor_set,
};
const _st_cmd_info ano_cmd_virtaul_stick_cmd = {
    .cmd.cmdid0 = 0,
    .cmd.cmdid1 = 1,
    .cmd.cmdid2 = 0,
    .name = "virtual to motor",
    .info = "Left: VAL0, Right: VAL2, UP: VAL1, DOWN: VAL3",
    .pFun = ano_cmd_virtaul_stick,
};

void ano_cmd_init(void)
{
    AnoPTv8CmdRegister(&ano_motor_on_cmd);
    AnoPTv8CmdRegister(&ano_motor_off_cmd);
    AnoPTv8CmdRegister(&ano_motor_set_cmd);
    AnoPTv8CmdRegister(&ano_cmd_virtaul_stick_cmd);
}
